dis_select_UWB= select(bag_UWB, 'Time',[bag_UWB.StartTime bag_UWB.EndTime], 'Topic', '/nlink_linktrack_nodeframe3');
disMsgs_UWB= readMessages(dis_select_UWB,'DataFormat','struct');

dis(:,1) = cellfun(@(m) double(m.Nodes(1).Dis),disMsgs_UWB);
dis(:,2) = cellfun(@(m) double(m.Nodes(2).Dis),disMsgs_UWB);
dis(:,3) = cellfun(@(m) double(m.Nodes(3).Dis),disMsgs_UWB);
for i=1:3541
    %i=5时，丢了id[2]
    %i=477时，丢了id[0]
    %i=2767时，丢了id[2]
    %i=3345时，丢了id[0]
    %i=3451时，丢了id[2]
    if i==5 || i==2767 ||i==3451  
        dis(i,4)=dis(i,3);
        dis(i,3)=dis(i-1,3);
        continue;
    end
    if i==477||i==3345  
        dis(i,4)=dis(i,1);
        dis(i,1)=dis(i-1,1);
        continue;
    end
    dis(i,4)=disMsgs_UWB{i}.Nodes(4).Dis;
end
dis_old=dis;
t = cellfun(@(m) double(m.SystemTime),disMsgs_UWB);
t_relative=(t-t(1))/1000;

x0=0;y0=0;z0=0;
x1=x0+0.95;
y1=y0;
z1=z0;
x2=x0;
y2=y0+0.7;
z2=z0;
x3=x0+0.95;
y3=y0+0.7;
z3=z0;

%%
%均值滤波
dis_mean=dis;
for i=1:4
    for j=2:3541
        dis_mean(j,i)=0.9*dis_mean(j-1,i)+0.1*dis_mean(j,i);
    end
end
%%
%sg滤波
dis_sg=sgolayfilt(dis,3,11);
%%
%butter滤波
dis_butter=filter(filter_fun,dis);
%%
%改进版最小二乘
p_new_butter=least_square_new(dis_butter,3541);
p_new_mean=least_square_new(dis_mean,3541);
p_new_ori=least_square_new(dis,3541);
p_new_sg=least_square_new(dis_sg,3541);
p_new_sg_3_11=sgolayfilt(p_new_sg,3,11);
p_new_sg_3_31=sgolayfilt(p_new_sg,3,31);
p_new_sg_7_31=sgolayfilt(p_new_sg,7,31);
p_new_sg_3_101=sgolayfilt(p_new_sg,3,101);
%%
%最小二乘
p_old_butter=least_square_old(dis_butter,3541);
p_old_mean=least_square_old(dis_butter,3541);
p_old_ori=least_square_old(dis,3541);

%284 337 357 477 1210
%48.4- -5.2  54.92 -2.54893 
